#!/usr/bin/env python3
# encoding: utf-8
# @data:2023/09/27
# @author:aiden

import rospy
import signal
import time
from ainex_sdk import misc, common
from ainex_example.color_common import Common
from ainex_example.visual_patrol import VisualPatrol
from ainex_example.approach_object import ApproachObject
from ainex_interfaces.srv import SetString
from ainex_interfaces.msg import ObjectsInfo, ColorDetect, ROI


class CompetitionNode(Common):
    # 按顺序检测三个roi，如果检测到黑线立刻跳出
    # y_min, y_max, x_min, x_max分别表示占图像的比例, 即实际大小为y_min*height
    # 优化检测区域，增强远距离和转弯后的黑线识别能力
    # ROI区域划分说明：
    # - Y轴（垂直）: 0=图像顶部, 1=图像底部，值越小越远离机器人
    # - X轴（水平）: 0=图像左侧, 1=图像右侧，0.5=图像中央
    # - 检测顺序：从远到近（上区域→中区域→下区域）
    line_roi = [
        (2 / 12, 4 / 12, 1 / 4, 3 / 4),  # 上区域：Y=17%-33%，扩大远距离检测，适应转弯
        (4 / 12, 5.5 / 12, 3 / 8, 5 / 8),  # 中区域：Y=33%-46%，主要跟踪区域
        (
            4.5 / 12,
            5.5 / 12,
            2 / 5,
            3 / 5,
        ),  # 下区域：Y=37.5%-46%，近距离精确跟踪
    ]

    stairs_roi = [1 / 8, 1, 0, 1]
    hurdles_roi = [1 / 8, 1, 0.15, 0.85]
    # 分离左右检测区域，避免误识别手上的方块
    block_left_roi = [
        1 / 12,
        350 / 480,
        1 / 12,
        7 / 12,  # X轴只检测左半部分：12.5%-58%
    ]  # 左抓取专用ROI：只检测画面左半部分的方块
    block_right_roi = [
        1 / 12,
        350 / 480,
        5 / 12,  # X轴只检测右半部分：42%-87.5%
        11 / 12,
    ]  # 右抓取专用ROI：只检测画面右半部分的方块
    # 保持原有的通用block_roi，用于初始化
    block_roi = [
        1 / 12,
        350 / 480,
        1 / 12,
        11 / 12,
    ]  # 缩小检测区域：Y=17%-73%，X=12.5%-87.5%，减少误检
    intersection_roi = [
        80 / 480,
        200 / 480,
        0.15,
        0.85,
    ]  # 优化ROI：更专注于横线检测区域

    # 所需动作的名称
    hurdles_action_name = "hurdles"
    climb_stairs_action_name = "climb_stairs"
    descend_stairs_action_name = "descend_stairs"
    crawl_left_action_name = "crawl_left"  # 左行走动作名称
    crawl_right_action_name = "crawl_right"  # 右行走动作名称
    place_block_action_name = "place_block"  # 放置方块动作名称
    forward_step = "forward_one_step"  # 前进一步
    back_step = "back_step"  # 后退一步
    move_left = "move_left"  # 左移
    move_right = "move_right"  # 右移
    turn_left = "turn_left_30"  # 左转
    turn_right = "turn_right_30"  # 右转

    # 图像处理时缩放到这个分辨率， 不建议修改
    image_process_size = [160, 120]
    # 上台阶状态下的目标位置阈值
    enter_climb_stairs_y = (
        350 / 480
    )  # 当检测到的标识像素坐标y值占图像的比例大于此值时进入此阶段
    climb_stairs_x_stop = 0.5  # 当检测到的标识像素坐标x值占图像的比例在此值附近(范围可在ApproachObject里设置)时停止前后移动
    climb_stairs_y_stop = (
        350 / 480
    )  # 当检测到的标识像素坐标y值占图像的比例在此值附近(范围可在ApproachObject里设置)时停止横向移动
    climb_stairs_yaw_stop = (
        0  # 当检测到的标识角度在此值附近(范围可在ApproachObject里设置)时停止旋转移动
    )
    # 下台阶状态下的目标位置阈值
    enter_descend_stairs_y = 285 / 480
    descend_stairs_x_stop = 0.45
    descend_stairs_y_stop = 285 / 480
    descend_stairs_yaw_stop = 0
    # 跨栏状态下的目标位置阈值
    enter_hurdles_y = 270 / 480
    hurdles_x_stop = 0.5
    hurdles_y_stop = 270 / 480
    hurdles_yaw_stop = 0
    # 向左抓取状态下的目标位置阈值
    enter_crawl_left_y = (
        60 / 480
    )  # 当检测到的标识像素坐标y值占图像的比例大于此值时进入此阶段
    crawl_left_x_stop = (
        225 / 640
    )  # 左行走的x轴目标位置：调整为更左的位置(200px而非240px)，让机器人在方块位于左侧时停止
    crawl_left_y_stop = (
        60 / 480
    )  # 抓取时停止前进的Y位置阈值：50px处停止，开始执行抓取动作 左行走的y轴目标位置(范围可在ApproachObject里设置)时停止横向移动
    crawl_left_yaw_stop = (
        0  # 当检测到的标识角度在此值附近(范围可在ApproachObject里设置)时停止旋转移动
    )
    # 向右抓取状态下的目标位置阈值，原理同上
    enter_crawl_right_y = 80 / 480
    crawl_right_x_stop = 400 / 640
    crawl_right_y_stop = 80 / 480
    crawl_right_yaw_stop = 0
    # 放块状态下的目标位置阈值
    enter_place_block_y = 150 / 480  # 检测横线的Y轴位置
    place_block_x_stop = 320 / 640
    place_block_y_stop = 150 / 480
    place_block_yaw_stop = 0

    def __init__(self, name):
        rospy.init_node(name)
        self.calib_config = common.get_yaml_data(
            "/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml"
        )
        self.name = name
        self.count = 0
        self.running = True
        self.slow = True
        self.objects_info = []
        self.delay_time = 0
        # 添加校准计数器，分别管理不同动作的校准次数
        self.hurdles_calibration_count = 0
        self.descend_stairs_calibration_count = 0
        self.max_calibration_attempts = 2  # 最大校准次数
        # 添加上台阶超时机制
        self.climb_stairs_timeout = 0
        self.climb_stairs_timeout_duration = 3.0  # 3秒超时
        self.climb_stairs_started = False
        self.current_state = "visual_patrol"  # 当前状态
        self.next_state = "hurdles"

        self.state = {
            "visual_patrol": [
                [500, 260],
                [
                    "black",
                    self.line_roi,
                    self.image_process_size,
                    self.set_visual_patrol_color,
                ],
                False,
            ],  # 巡线
            "climb_stairs": [
                [500, 260],
                [
                    "red",
                    self.stairs_roi,
                    self.image_process_size,
                    self.set_stairs_color,
                ],
                False,
            ],  # 上台阶
            "descend_stairs": [
                [500, 260],
                [
                    "red",
                    self.stairs_roi,
                    self.image_process_size,
                    self.set_stairs_color,
                ],
                False,
            ],  # 下台阶
            "hurdles": [
                [500, 260],
                [
                    "blue",
                    self.hurdles_roi,
                    self.image_process_size,
                    self.set_hurdles_color,
                ],
                False,
            ],  # 跨栏
            "crawl_left": [
                [500, 260],
                [
                    "green",
                    self.block_left_roi,  # 使用左侧专用ROI
                    self.image_process_size,
                    self.set_block_color,
                ],
                False,
            ],  # 左抓取状态
            "crawl_right": [
                [500, 260],
                [
                    "green",
                    self.block_right_roi,  # 使用右侧专用ROI
                    self.image_process_size,
                    self.set_block_color,
                ],
                False,
            ],  # 右抓取
            "place_block": [
                [500, 260],
                [
                    "black",
                    self.intersection_roi,
                    self.image_process_size,
                    self.set_intersection_color,
                ],
                False,
            ],
        }  # 放置

        self.head_pan_init = self.state[self.current_state][0][0]  # 左右舵机的初始值
        self.head_tilt_init = self.state[self.current_state][0][1]  # 上下舵机的初始值
        super().__init__(name, self.head_pan_init, self.head_tilt_init)

        self.approach_object = ApproachObject(self.gait_manager)
        self.visual_patrol = VisualPatrol(self.gait_manager)
        self.visual_patrol.yaw_range = [-12, 12]
        # 应用参考speed==4的稳定配置
        self.apply_stable_gait_params()

        signal.signal(signal.SIGINT, self.shutdown)

        # 订阅颜色识别结果
        rospy.Subscriber("/object/pixel_coords", ObjectsInfo, self.get_color_callback)
        rospy.Service("~set_color", SetString, self.set_color_srv_callback)  # 设置颜色
        self.motion_manager.run_action("walk_ready")

        if rospy.get_param("~start", True):
            # 通知颜色识别准备，此时只显示摄像头原画
            self.enter_func(None)
            self.start_srv_callback(None)
            common.loginfo("start ombination")

    def shutdown(self, signum, frame):
        self.running = False
        common.loginfo("%s shutdown" % self.name)

    def apply_stable_gait_params(self, go_x_max=0.020, turn_x_max=0.018, arm_swap=0.05):
        """应用参考speed==4的稳定步态参数"""

        go_param = self.gait_manager.get_gait_param()
        go_param["angle_move_amplitude"] = -4.0
        go_param["step_height"] = 0.018  # z_move_amplitude
        go_param["z_swap_amplitude"] = 0.004
        go_param["init_roll_offset"] = -5.0
        go_param["step_fb_ratio"] = 0.028
        # 添加右脚踝校正参数，解决往左偏移问题
        go_param["init_y_offset"] = -0.004  # 右脚向内调整2mm，补偿踝关节偏移
        go_param["pelvis_offset"] = 2  # 减小髋关节摆动，提高稳定性
        self.visual_patrol.update_go_gait(
            dsp=[280, 0.18, 0.025], x_max=go_x_max, walking_param=go_param
        )

        turn_param = self.gait_manager.get_gait_param()
        turn_param["angle_move_amplitude"] = -4.0
        turn_param["step_height"] = 0.020  # 转弯时增加步高，提高转弯效果
        turn_param["z_swap_amplitude"] = 0.005  # 增加摆动幅度
        turn_param["init_roll_offset"] = -5.0
        turn_param["step_fb_ratio"] = 0.020  # 减小前后比例，增加侧向转弯
        # 转弯时也应用相同的校正
        turn_param["init_y_offset"] = -0.004
        turn_param["pelvis_offset"] = 4  # 转弯时稍微增加髋关节摆动
        self.visual_patrol.update_turn_gait(
            dsp=[250, 0.15, 0.03],  # 转弯时更快步频、更小双足支撑、更大摆幅
            x_max=turn_x_max,
            arm_swap=arm_swap,
            walking_param=turn_param,
        )

        # 确保转弯角度范围在每次参数恢复后都正确设置
        self.visual_patrol.yaw_range = [-14, 14]

    def set_visual_patrol_color(self, color, roi, image_process_size):
        # 设置巡线颜色
        line_param = ColorDetect()
        line_param.color_name = color
        line_param.use_name = True
        line_param.detect_type = "line"
        line_param.image_process_size = image_process_size
        line_param.line_roi.up.y_min = int(roi[0][0] * image_process_size[1])
        line_param.line_roi.up.y_max = int(roi[0][1] * image_process_size[1])
        line_param.line_roi.up.x_min = int(roi[0][2] * image_process_size[0])
        line_param.line_roi.up.x_max = int(roi[0][3] * image_process_size[0])

        line_param.line_roi.center.y_min = int(roi[1][0] * image_process_size[1])
        line_param.line_roi.center.y_max = int(roi[1][1] * image_process_size[1])
        line_param.line_roi.center.x_min = int(roi[1][2] * image_process_size[0])
        line_param.line_roi.center.x_max = int(roi[1][3] * image_process_size[0])

        line_param.line_roi.down.y_min = int(roi[2][0] * image_process_size[1])
        line_param.line_roi.down.y_max = int(roi[2][1] * image_process_size[1])
        line_param.line_roi.down.x_min = int(roi[2][2] * image_process_size[0])
        line_param.line_roi.down.x_max = int(roi[2][3] * image_process_size[0])

        line_param.min_area = 5
        line_param.max_area = image_process_size[0] * image_process_size[1]

        return line_param

    # 生成方块颜色识别参数
    def set_block_color(self, color, roi, image_process_size):
        block_param = ColorDetect()
        block_param.color_name = color
        block_param.detect_type = "circle"
        block_param.use_name = True
        block_param.image_process_size = self.image_process_size
        block_param.roi.y_min = int(roi[0] * image_process_size[1])
        block_param.roi.y_max = int(roi[1] * image_process_size[1])
        block_param.roi.x_min = int(roi[2] * image_process_size[0])
        block_param.roi.x_max = int(roi[3] * image_process_size[0])
        block_param.min_area = 50  # 增加最小面积：从10改为50，减少误检
        block_param.max_area = image_process_size[0] * image_process_size[1]

        return block_param

    # 生成交叉点颜色识别参数
    def set_intersection_color(self, color, roi, image_process_size):
        intersection_param = ColorDetect()
        intersection_param.color_name = color
        intersection_param.detect_type = "intersection"
        intersection_param.use_name = True
        intersection_param.image_process_size = image_process_size
        intersection_param.roi.y_min = int(roi[0] * image_process_size[1])
        intersection_param.roi.y_max = int(roi[1] * image_process_size[1])
        intersection_param.roi.x_min = int(roi[2] * image_process_size[0])
        intersection_param.roi.x_max = int(roi[3] * image_process_size[0])
        intersection_param.min_area = 5
        intersection_param.max_area = image_process_size[0] * image_process_size[1]

        return intersection_param

    def set_stairs_color(self, color, roi, image_process_size):
        # 设置台阶标志颜色
        stairs_param = ColorDetect()
        stairs_param.color_name = color
        stairs_param.detect_type = "side"
        stairs_param.use_name = True
        stairs_param.image_process_size = image_process_size
        stairs_param.roi.y_min = int(roi[0] * image_process_size[1])
        stairs_param.roi.y_max = int(roi[1] * image_process_size[1])
        stairs_param.roi.x_min = int(roi[2] * image_process_size[0])
        stairs_param.roi.x_max = int(roi[3] * image_process_size[0])
        stairs_param.min_area = 10 * 20
        stairs_param.max_area = image_process_size[0] * image_process_size[1]

        return stairs_param

    def set_hurdles_color(self, color, roi, image_process_size):
        # 设置台阶标志颜色
        hurdles_param = ColorDetect()
        hurdles_param.color_name = color
        hurdles_param.detect_type = "side"
        hurdles_param.use_name = True
        hurdles_param.image_process_size = image_process_size
        hurdles_param.roi.y_min = int(roi[0] * image_process_size[1])
        hurdles_param.roi.y_max = int(roi[1] * image_process_size[1])
        hurdles_param.roi.x_min = int(roi[2] * image_process_size[0])
        hurdles_param.roi.x_max = int(roi[3] * image_process_size[0])
        hurdles_param.min_area = 10 * 20
        hurdles_param.max_area = image_process_size[0] * image_process_size[1]

        return hurdles_param

    def set_color_srv_callback(self, msg):
        # 设置颜色
        block_param = self.set_block_color(self.state["crawl_left"][1][0])
        stairs_param = self.set_stairs_color(self.state["climb_stairs"][1][0])
        line_param = self.set_visual_patrol_color(self.state["visual_patrol"][1][0])
        hurdles_param = self.set_hurdles_color(self.state["hurdles"][1][0])
        intersection_param = self.set_intersection_color(
            self.state["place_block"][1][0]
        )

        self.detect_pub.publish(
            [line_param, stairs_param, hurdles_param, intersection_param, block_param]
        )
        common.loginfo("%s set_color" % self.name)

        return [True, "set_color"]

    def get_color_callback(self, msg):
        # 获取颜色识别结果
        self.objects_info = msg.data

    def state_init(self, current_state, next_state):
        # 不同阶段的初始化
        if self.state[current_state][2] == False:
            self.state[current_state][2] = True
            self.init_action(
                self.state[current_state][0][0], self.state[current_state][0][1]
            )  # 头部姿态
            param1 = self.state[current_state][1][3](
                self.state[current_state][1][0],
                self.state[current_state][1][1],
                self.state[current_state][1][2],
            )
            param2 = self.state[next_state][1][3](
                self.state[next_state][1][0],
                self.state[next_state][1][1],
                self.state[next_state][1][2],
            )
            self.detect_pub.publish([param1, param2])  # 颜色检测设置
            common.loginfo(current_state + " init")

    # 退出上台阶
    def exit_climb_stairs(self, stairs_data):
        # 上阶梯处理 - 添加超时机制
        # 首次进入时，启动超时计时
        if not self.climb_stairs_started:
            self.climb_stairs_started = True
            self.climb_stairs_timeout = time.time() + self.climb_stairs_timeout_duration
            print(f"Climb stairs started, timeout will trigger at {self.climb_stairs_timeout_duration}s")
        
        # 检查是否超时
        timeout_triggered = time.time() > self.climb_stairs_timeout
        
        if stairs_data is not None:
            print("state:exit_climb_stairs ", "stairs_data.y:", stairs_data.y)
            # 如果检测到目标，立即执行动作
            self.gait_manager.disable()  # 关闭步态控制
            common.loginfo("climb_stairs - executing with detected data")
            self.motion_manager.run_action(
                self.climb_stairs_action_name
            )  # 执行上台阶动作
            rospy.sleep(0.5)
            # 恢复到参考speed==4的稳定巡线配置 - 修复遗漏的参数恢复
            self.apply_stable_gait_params()
            self.slow = True
            self.motion_manager.run_action("walk_ready")
            rospy.sleep(0.5)
            # 重置超时状态
            self.climb_stairs_started = False
            return True
        elif timeout_triggered:
            # 超时触发，无法继续检测，直接执行动作
            print(f"Climb stairs timeout triggered after {self.climb_stairs_timeout_duration}s without target detection")
            common.loginfo("climb_stairs - executing after timeout (no detection)")
            self.gait_manager.disable()  # 关闭步态控制
            self.motion_manager.run_action(
                self.climb_stairs_action_name
            )  # 执行上台阶动作
            rospy.sleep(0.5)
            # 恢复到参考speed==4的稳定巡线配置 - 修复遗漏的参数恢复
            self.apply_stable_gait_params()
            self.slow = True
            self.motion_manager.run_action("walk_ready")
            rospy.sleep(0.5)
            # 重置超时状态
            self.climb_stairs_started = False
            return True
        
        return False

    # 退出下台阶
    def exit_descend_stairs(self, stairs_data):
        # 下阶梯处理 - 强制校准逻辑
        print(
            "state:exit_descend_stairs ",
            "stairs_data:",
            stairs_data is not None,
            "calibration_count:",
            self.descend_stairs_calibration_count,
        )

        # 如果校准次数未超过最大值，强制执行校准逻辑
        if self.descend_stairs_calibration_count < self.max_calibration_attempts:
            self.descend_stairs_calibration_count += 1

            if stairs_data is not None:
                # 有数据时进行校准
                calibration_result = self.approach_object.process(
                    max(
                        stairs_data.y,
                        stairs_data.left_point[1],
                        stairs_data.right_point[1],
                    ),
                    stairs_data.x + self.calib_config["center_x_offset"],
                    stairs_data.angle,
                    self.descend_stairs_y_stop * stairs_data.height,
                    self.descend_stairs_x_stop * stairs_data.width,
                    self.descend_stairs_yaw_stop,
                    stairs_data.width,
                    stairs_data.height,
                )
                common.loginfo(
                    "descend_stairs calibration attempt {}/{} - with data".format(
                        self.descend_stairs_calibration_count,
                        self.max_calibration_attempts,
                    )
                )
            else:
                # 无数据时也继续校准流程，使用默认行为
                calibration_result = False  # 强制继续校准
                common.loginfo(
                    "descend_stairs calibration attempt {}/{} - no data, continuing".format(
                        self.descend_stairs_calibration_count,
                        self.max_calibration_attempts,
                    )
                )

            # 不管校准结果如何，都继续校准流程直到达到最大次数
            return False

        else:
            # 达到最大校准次数，执行动作
            common.loginfo(
                "descend_stairs - completing calibration after {} attempts, executing action".format(
                    self.max_calibration_attempts
                )
            )
            self.gait_manager.disable()  # 关闭步态控制
            self.motion_manager.run_action(self.forward_step)
            self.motion_manager.run_action(
                self.descend_stairs_action_name
            )  # 执行下台阶动作
            rospy.sleep(0.5)
            # 恢复到参考speed==4的稳定巡线配置
            self.apply_stable_gait_params()
            self.slow = True
            self.motion_manager.run_action("walk_ready")
            rospy.sleep(0.5)
            # 重置计数器
            self.descend_stairs_calibration_count = 0
            return True

    # 退出跨栏
    def exit_hurdles(self, hurdles_data):
        # 跨栏处理 - 强制校准逻辑
        print(
            "state:exit_hurdles ",
            "hurdles_data:",
            hurdles_data is not None,
            "calibration_count:",
            self.hurdles_calibration_count,
        )

        # 如果校准次数未超过最大值，强制执行校准逻辑
        if self.hurdles_calibration_count < self.max_calibration_attempts:
            self.hurdles_calibration_count += 1

            if hurdles_data is not None:
                # 有数据时进行校准
                calibration_result = self.approach_object.process(
                    max(
                        hurdles_data.y,
                        hurdles_data.left_point[1],
                        hurdles_data.right_point[1],
                    ),
                    hurdles_data.x + self.calib_config["center_x_offset"],
                    hurdles_data.angle,
                    self.hurdles_y_stop * hurdles_data.height,
                    self.hurdles_x_stop * hurdles_data.width,
                    self.hurdles_yaw_stop,
                    hurdles_data.width,
                    hurdles_data.height,
                )
                common.loginfo(
                    "hurdles calibration attempt {}/{} - with data".format(
                        self.hurdles_calibration_count, self.max_calibration_attempts
                    )
                )
            else:
                # 无数据时也继续校准流程，使用默认行为
                calibration_result = False  # 强制继续校准
                common.loginfo(
                    "hurdles calibration attempt {}/{} - no data, continuing".format(
                        self.hurdles_calibration_count, self.max_calibration_attempts
                    )
                )

            # 不管校准结果如何，都继续校准流程直到达到最大次数
            return False

        else:
            # 达到最大校准次数，执行动作
            common.loginfo(
                "hurdles - completing calibration after {} attempts, executing action".format(
                    self.max_calibration_attempts
                )
            )
            self.gait_manager.disable()
            self.motion_manager.run_action(self.hurdles_action_name)
            rospy.sleep(0.5)
            # 恢复到参考speed==4的稳定巡线配置
            self.apply_stable_gait_params()
            self.slow = True
            self.motion_manager.run_action("walk_ready")
            rospy.sleep(0.5)
            # 重置计数器
            self.hurdles_calibration_count = 0
            return True

    # 进入上台阶
    def enter_climb_stairs(self, stairs_data):
        if stairs_data is not None:
            print("state:enter_climb_stairs ", "stairs_data.y:", stairs_data.y)
            if stairs_data.y > self.stairs_roi[0] * stairs_data.height and self.slow:
                self.apply_stable_gait_params()
                self.slow = False
            if (
                max(
                    stairs_data.y, stairs_data.left_point[1], stairs_data.right_point[1]
                )
                > self.enter_climb_stairs_y * stairs_data.height
            ):
                self.count += 1
                if (
                    self.count > 5
                ):  # 主线程比较快，颜色检测回调慢一点，需要连续检测来排除滞后干扰
                    self.count = 0
                    self.gait_manager.disable()
                    self.approach_object.update_approach_stop_value(
                        20, 30, 6
                    )  # 设置靠近目标停止的条件，分别为y, x, angle误差
                    self.motion_manager.run_action("hand_back")  # 手往后，防止遮挡
                    rospy.sleep(0.5)
                    # 重置超时计时
                    self.climb_stairs_started = False
                    return True
            else:
                self.count = 0
        return False

    # 进入下台阶
    def enter_descend_stairs(self, stairs_data):
        if stairs_data is not None:
            print("state:enter_descend_stairs ", "stairs_data.y:", stairs_data.y)
            if (
                max(
                    stairs_data.y, stairs_data.left_point[1], stairs_data.right_point[1]
                )
                > self.enter_descend_stairs_y * stairs_data.height
            ):
                self.count += 1
                if self.count > 5:
                    self.count = 0
                    self.gait_manager.disable()
                    self.approach_object.update_approach_stop_value(20, 30, 6)
                    self.motion_manager.run_action("hand_back")  # 手往后，防止遮挡
                    rospy.sleep(0.5)
                    return True
            else:
                self.count = 0
        return False

    # 进入跨栏
    def enter_hurdles(self, hurdles_data):
        if hurdles_data is not None:
            print("state:enter_hurdles ", "hurdles_data.y:", hurdles_data.y)
            if hurdles_data.y > self.hurdles_roi[0] * hurdles_data.height and self.slow:
                self.apply_stable_gait_params()
                self.slow = False
            if (
                max(
                    hurdles_data.y,
                    hurdles_data.left_point[1],
                    hurdles_data.right_point[1],
                )
                > self.enter_hurdles_y * hurdles_data.height
            ):
                self.count += 1
                if self.count > 5:
                    self.count = 0
                    self.gait_manager.disable()
                    self.approach_object.update_approach_stop_value(
                        x_approach_value=30, yaw_approach_value=5
                    )
                    self.motion_manager.run_action("hand_back")  # 手往后，防止遮挡
                    return True
            else:
                self.count = 0
        return False

    # 进入左抓取判断函数
    def enter_crawl_left(self, block_data):
        if block_data is not None:
            print(
                "state:enter_crawl_left",
                " block_data.x:",
                block_data.x,
                " block_data.y:",
                block_data.y,
            )
            if block_data.y > self.block_roi[0] * block_data.height and self.slow:
                self.apply_stable_gait_params()
                self.slow = False
            # 使用参数控制进入抓取的距离，而不是硬编码值
            if (
                block_data.y > self.enter_crawl_left_y * block_data.height
            ):  # 使用参数：50/480 ≈ 50px
                self.count += 1
                if self.count > 8:  # 增加连续检测次数：从5次改为8次，减少误触发
                    self.count = 0
                    self.gait_manager.disable()
                    return True
            else:
                self.count = 0
        else:
            self.count = 0
        return False

    # 进入右抓取判断函数
    def enter_crawl_right(self, block_data):
        if block_data is not None:
            print(
                "state:enter_crawl_right",
                " block_data.x:",
                block_data.x,
                " block_data.y:",
                block_data.y,
            )
            if block_data.y > self.block_roi[0] * block_data.height and self.slow:
                self.apply_stable_gait_params()
                self.slow = False
            # 同步左抓取逻辑：使用参数控制进入抓取的距离
            if (
                block_data.y > self.enter_crawl_right_y * block_data.height
            ):  # 使用参数：100/480 ≈ 100px
                self.count += 1
                if self.count > 8:  # 同步左抓取逻辑：增加连续检测次数，减少误触发
                    self.count = 0
                    self.gait_manager.disable()
                    return True
            else:
                self.count = 0
        else:
            self.count = 0
        return False

    # 进入放置判断函数
    def enter_place_block(self, line_data):
        if line_data is not None:
            print("state:enter_place_block ", "line_data.y:", line_data.y)
            line_width = line_data.right_point[0] - line_data.left_point[0]
            line_height = abs(line_data.right_point[1] - line_data.left_point[1])
            print(f"线条宽度: {line_width}, 线条高度: {line_height}")

            # 强化横线识别条件：
            # 1. 线条宽度要足够大（横向延伸）
            # 2. 线条高度要相对较小（不是竖线）
            # 3. 宽高比要符合横线特征
            width_height_ratio = line_width / (line_height + 1)  # 避免除零

            if (
                max(line_data.y, line_data.left_point[1], line_data.right_point[1])
                > self.enter_place_block_y * line_data.height
                and line_width > 270  # 增加宽度要求：从270改为300
                and line_height < 80  # 限制高度：横线的高度应该较小
                and width_height_ratio > 3  # 宽高比：横线应该宽度远大于高度
            ):
                self.count += 1
                if self.count > 4:  # 增加连续检测次数，提高稳定性
                    self.count = 0
                    self.gait_manager.stop()
                    common.loginfo(
                        f"横线检测成功 - 宽度:{line_width}, 高度:{line_height}, 比例:{width_height_ratio:.2f}"
                    )
                    return True
            else:
                self.count = 0
        else:
            self.count = 0
        return False

    # 退出左抓取判断函数
    def exit_crawl_left(self, block_data):
        if block_data is not None:
            print(
                "state:exit_crawl_left",
                " block_data.x:",
                block_data.x,
                " block_data.y:",
                block_data.y,
            )
            centerx_left = self.crawl_left_x_stop * block_data.width
            if block_data.x - centerx_left > 15:
                self.motion_manager.run_action(self.move_right)
            elif block_data.x - centerx_left < -15:
                self.motion_manager.run_action(self.move_left)

                # 还没靠近目标，慢慢靠近
            elif block_data.y < self.crawl_left_y_stop * block_data.height:
                self.motion_manager.run_action(self.forward_step)
                time.sleep(0.2)

            elif block_data.y >= self.crawl_left_y_stop * block_data.height:
                time.sleep(0.5)
                if block_data.y >= self.crawl_left_y_stop * block_data.height:
                    self.motion_manager.run_action(self.crawl_left_action_name)

                    self.slow = True
                    self.motion_manager.run_action(self.turn_left)
                    self.motion_manager.run_action(self.move_right)

                    # 恢复到参考speed==4的稳定巡线配置
                    self.apply_stable_gait_params()

                    return True

        else:
            self.motion_manager.run_action(self.back_step)
            time.sleep(0.01)

        return False

    # 退出右抓取判断函数
    def exit_crawl_right(self, block_data):
        if block_data is not None:
            print(
                "state:exit_crawl_right",
                " block_data.x:",
                block_data.x,
                "block_data.y:",
                block_data.y,
            )
            centerx_right = self.crawl_right_x_stop * block_data.width
            print(
                f"右抓取 - 目标x位置: {centerx_right}, 当前方块x位置: {block_data.x}, 差值: {block_data.x - centerx_right}"
            )

            if block_data.x - centerx_right > 20:  # 方块在目标左侧，机器人需要向右移动
                self.motion_manager.run_action(self.move_right)

            elif (
                block_data.x - centerx_right < -20
            ):  # 方块在目标右侧，机器人需要向左移动
                self.motion_manager.run_action(self.move_left)
                # 还没靠近目标，慢慢靠近
            elif block_data.y < self.crawl_right_y_stop * block_data.height:
                self.motion_manager.run_action(self.forward_step)
                time.sleep(0.2)

            elif block_data.y >= self.crawl_right_y_stop * block_data.height:
                time.sleep(0.5)
                if block_data.y >= self.crawl_right_y_stop * block_data.height:
                    self.motion_manager.run_action(self.crawl_right_action_name)
                    self.slow = True
                    self.motion_manager.run_action(self.turn_right)
                    self.motion_manager.run_action(self.move_left)
                    self.motion_manager.run_action(self.turn_right)

                    # 恢复到参考speed==4的稳定巡线配置
                    self.apply_stable_gait_params()

                    self.delay_time = time.time() + 17  # 抓完色块延时后再识别横线

                    return True

        else:
            self.motion_manager.run_action(self.back_step)
            time.sleep(0.01)

        return False

    # 退出放块判断函数
    def exit_place_block(self, line_data):
        if line_data is not None:
            print("state:exit_place_block ", "line_data.y:", line_data.y)
            # if self.approach_object.process(
            #     max(line_data.y, line_data.left_point[1], line_data.right_point[1]),
            #     line_data.x,
            #     line_data.angle,
            #     self.place_block_y_stop * line_data.height,
            #     self.place_block_x_stop * line_data.width,
            #     self.place_block_yaw_stop,
            #     line_data.width,
            #     line_data.height,
            # ):
            self.gait_manager.disable()  # 关闭步态控制
            walking_param = self.gait_manager.get_gait_param()  # 获取当前的步态参数
            walking_param["body_height"] = 0.015  # 设置身体高度,单位米
            walking_param["pelvis_offset"] = 7  # 设置骨盆位置的前后偏移量,单位度
            walking_param["step_height"] = 0.01  # 设置步高,单位米
            walking_param["hip_pitch_offset"] = 20  # 设置髋关节角度偏移量,单位度
            walking_param["z_swap_amplitude"] = (
                0.006  # 设置左右足高度交替时的振幅,单位米
            )
            # 调用set_step 设置步态参数,包括步长、步频等
            self.gait_manager.set_step([400, 0.2, 0.02], 0.02, 0, 0, None, 0, 3)
            common.loginfo("place_block")
            self.motion_manager.run_action(self.place_block_action_name)
            return True
        return False

    def run(self):
        while self.running:
            if self.start:
                # 获取识别结果
                line_data = None
                side_data = None
                block_data = None
                intersection_data = None
                for object_info in self.objects_info:
                    if object_info.type == "line":
                        line_data = object_info
                    if object_info.type == "side":
                        side_data = object_info
                    if object_info.type == "circle":
                        block_data = object_info
                    if object_info.type == "intersection":
                        intersection_data = object_info
                    # print(object_info)
                # 当前阶段处理完成，回到巡线

                if self.current_state == "visual_patrol":
                    if line_data is not None:
                        self.visual_patrol.process(line_data.x, line_data.width)
                elif self.current_state == "hurdles":
                    if self.exit_hurdles(side_data):
                        self.current_state = "visual_patrol"
                        self.next_state = "climb_stairs"
                        self.state[self.current_state][2] = False  # 重新初始化当前阶段
                        common.loginfo("exit hurdles ---> enter visual_patrol")
                    else:
                        rospy.sleep(0.8)

                if self.current_state == "visual_patrol":
                    if line_data is not None:
                        self.visual_patrol.process(line_data.x, line_data.width)
                elif self.current_state == "climb_stairs":
                    if self.exit_climb_stairs(side_data):
                        self.current_state = "visual_patrol"
                        self.next_state = "descend_stairs"
                        self.state[self.current_state][2] = False  # 重新初始化当前阶段
                        common.loginfo("exit climb_stairs ---> enter visual_patrol")
                    else:
                        rospy.sleep(0.8)

                elif self.current_state == "descend_stairs":
                    if self.exit_descend_stairs(side_data):
                        self.current_state = "visual_patrol"
                        self.next_state = "crawl_left"
                        self.state[self.current_state][2] = False
                        common.loginfo("exit descend_stairs ---> enter visual_patrol")
                    else:
                        rospy.sleep(0.8)

                elif self.current_state == "crawl_left":
                    # 如果当前状态是左抓取
                    if self.exit_crawl_left(block_data):
                        # 如果完成左抓取
                        self.current_state = "visual_patrol"
                        self.next_state = "crawl_right"
                        self.state[self.current_state][2] = False
                    else:
                        rospy.sleep(0.8)  # 等机体平稳下来

                elif self.current_state == "crawl_right":
                    # 右抓取状态
                    if self.exit_crawl_right(block_data):
                        # 如果完成右抓取
                        self.current_state = "visual_patrol"
                        self.next_state = "place_block"
                        self.state[self.current_state][2] = False
                    else:
                        rospy.sleep(0.8)

                elif self.current_state == "place_block":
                    if self.exit_place_block(intersection_data):
                        self.running = False
                        common.loginfo("exit place_block ---> enter visual_patrol")
                    else:
                        rospy.sleep(0.8)

                # 是否退出巡线，进入下一阶段
                if self.next_state == "hurdles":
                    if self.enter_hurdles(side_data):
                        self.current_state = "hurdles"
                        self.next_state = "visual_patrol"
                        common.loginfo("exit visual_patrol ---> enter hurdles")
                elif self.next_state == "climb_stairs":
                    if self.enter_climb_stairs(side_data):
                        self.current_state = "climb_stairs"
                        self.next_state = "visual_patrol"
                        common.loginfo("exit visual_patrol ---> enter climb_stairs")
                elif self.next_state == "descend_stairs":
                    if self.enter_descend_stairs(side_data):
                        self.current_state = "descend_stairs"
                        self.next_state = "visual_patrol"
                        common.loginfo("exit visual_patrol ---> enter descend_stairs")

                elif self.next_state == "crawl_left":
                    if self.enter_crawl_left(block_data):
                        self.current_state = "crawl_left"
                        self.next_state = "visual_patrol"
                        common.loginfo("exit visual_patrol ---> enter crawl_left")
                elif self.next_state == "crawl_right":
                    # ROI分离后，无需延时检查，直接检测右侧区域的方块
                    if self.enter_crawl_right(block_data):
                        self.current_state = "crawl_right"
                        self.next_state = "visual_patrol"
                        common.loginfo("exit visual_patrol ---> enter crawl_right")
                elif self.next_state == "place_block":
                    # print(time.time(),self.delay_time)
                    if time.time() > self.delay_time:
                        if self.enter_place_block(intersection_data):
                            self.current_state = "place_block"
                            self.next_state = "visual_patrol"

                self.state_init(self.current_state, self.next_state)

                rospy.sleep(0.01)  # 防止空载
            else:
                rospy.sleep(0.01)

        self.init_action(self.head_pan_init, self.head_tilt_init)
        self.stop_srv_callback(None)
        rospy.signal_shutdown("shutdown")


if __name__ == "__main__":
    CompetitionNode("competition").run()
